#pragma once
#include <fstream>
#include <string>
#include <map>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/ml.hpp>
#include <opencv2/features2d.hpp>



void drawBothMatches(const cv::Mat left_image, const cv::Mat right_image, const std::vector<cv::Point2f> left_clusters1, const std::vector<cv::Point2f> left_clusters2, std::vector<cv::Point2f> right_clusters1, const std::vector<cv::Point2f> right_clusters2/*, std::string left_save_path, std::string right_save_path*/) {
    cv::Mat left_rgb_result = left_image.clone();
    for (int i = 0; i < left_clusters1.size(); i++) {
        cv::line(left_rgb_result, left_clusters1[i], left_clusters2[i], cv::Scalar(255, 0, 0), 2);
        cv::circle(left_rgb_result, left_clusters1[i], 2, cv::Scalar(0, 0, 255), -1);
        cv::circle(left_rgb_result, left_clusters2[i], 1, cv::Scalar(0, 255, 0), -1);
        // cv::imshow("Left", left_rgb_result);
        // cv::waitKey(0);
    }
    // cv::imwrite(left_save_path, left_rgb_result);

    cv::Mat right_rgb_result = right_image.clone();
    for (int i = 0; i < right_clusters1.size(); i++) {
        cv::line(right_rgb_result, right_clusters1[i], right_clusters2[i], cv::Scalar(255, 0, 0), 2);
        cv::circle(right_rgb_result, right_clusters1[i], 2, cv::Scalar(0, 0, 255), -1);
        cv::circle(right_rgb_result, right_clusters2[i], 1, cv::Scalar(0, 255, 0), -1);
        // cv::imshow("Right", right_rgb_result);
        // cv::waitKey(0);
    }
    // cv::imwrite(right_save_path, right_rgb_result);

    std::cout << std::endl;

    cv::namedWindow("Detection Results", cv::WINDOW_NORMAL);
    cv::resizeWindow("Detection Results", 1600, 900);
    cv::Mat result;
    cv::hconcat(left_rgb_result, right_rgb_result, result);

    int left_image_width = left_image.cols;
    for (int i = 0; i < left_clusters1.size(); i++) {
        // Create new points for the right image with the offset
        cv::Point2f right_point1 = right_clusters1[i] + cv::Point2f((float)left_image_width, 0);
        cv::Point2f right_point2 = right_clusters2[i] + cv::Point2f((float)left_image_width, 0);

        // Draw lines between corresponding points across the left and right images
        cv::line(result, left_clusters1[i], right_point1, cv::Scalar(0, 255, 255), 1); // Use a different color for clarity
        cv::line(result, left_clusters2[i], right_point2, cv::Scalar(0, 255, 255), 1); // Use a different color for clarity
    }
    cv::imshow("Detection Results", result);
    /*cv::imwrite("Detection_Result.png", result);*/
    cv::waitKey(1);
}


void combine_data(std::vector<cv::Point2d>& t1, std::vector<cv::Point2d>& t2, std::vector<cv::Point2f>& t3)
{
    for (auto p : t1)
    {
        t3.push_back(cv::Point2f(p.x,p.y));
    }
    for (auto p : t2)
    {
        t3.push_back(cv::Point2f(p.x, p.y));
    }
}


/*
std::vector<cv::Point2f>hand_machine1;
        std::vector<cv::Point2f>hand_machine2;
        std::vector<cv::Point2f>cluster_locator1;
        std::vector<cv::Point2f>cluster_locator2;

        combine_data(Leftcamtopose.cluster_hand_machine_up_matched_l, Leftcamtopose.cluster_hand_machine_down_matched_l, hand_machine1);
        combine_data(Leftcamtopose.cluster_locator_up_matched_l, Leftcamtopose.cluster_locator_down_matched_l, hand_machine1);
        combine_data(Rightcamtopose.cluster_hand_machine_up_matched_R, Rightcamtopose.cluster_hand_machine_down_matched_R, cluster_locator1);
        combine_data(Rightcamtopose.cluster_locator_up_matched_R, Rightcamtopose.cluster_locator_down_matched_R, cluster_locator2);
        drawBothMatches(Leftcamtopose.Image, Rightcamtopose.Image, hand_machine1, hand_machine2, cluster_locator1, cluster_locator2);


*/